/* Copyright (c) 2009 Mark L. Howison
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  (1) Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *  (2) Redistributions in binary form must reproduce the above copyright
 *      notice, this list of conditions and the following disclaimer in the
 *      documentation and/or other materials provided with the distribution.
 *  (3) The name of the copyright holder may not be used to endorse or promote
 *      products derived from this software without specific prior written
 *      permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

package edu.berkeley.edrl.wiikinemathics;

import java.util.ArrayList;

/**
 *
 * @author mhowison
 */
public class MotionDriver {

    private ArrayList<Double> waypointsLeft = new ArrayList(16);
    private ArrayList<Double> waypointsRight = new ArrayList(16);
    private double[] timesLeft, timesRight, times;
    private double[] multipliers;
    private double[] cumulative;
    private int iLeft, iRight;
    private double correction;

    private boolean getWaypoints() {
        waypointsLeft.clear();
        waypointsRight.clear();
        int rows = WiiKinemathicsApp.getApplication().driverPanel.input.getRowCount();
        try {
            for (int i=0; i<rows; i++) {
                Object cell;
                cell = WiiKinemathicsApp.getApplication().driverPanel.input.getValueAt(i,0);
                if (cell != null) {
                    String line = cell.toString();
                    if (line.length() > 0) {
                        double d = Double.valueOf(line);
                        if (WiiKinemathicsApp.getControls().roundDriverOption.isSelected()) {
                            d = Math.round(d);
                            WiiKinemathicsApp.getApplication().driverPanel.input.setValueAt(new Integer((int)d), i, 0);
                        }
                        waypointsLeft.add(d);
                    }
                }
                cell = WiiKinemathicsApp.getApplication().driverPanel.input.getValueAt(i,1);
                if (cell != null) {
                    String line = cell.toString();
                    if (line.length() > 0) {
                        double d = Double.valueOf(line);
                        if (WiiKinemathicsApp.getControls().roundDriverOption.isSelected()) {
                            d = Math.round(d);
                            WiiKinemathicsApp.getApplication().driverPanel.input.setValueAt(new Integer((int)d), i, 1);
                        }
                        waypointsRight.add(d);
                    }
                }
            }
        } catch (NumberFormatException e) {
            return false;
        }
        return true;
    }

    private void init() {
        double ratio = WiiKinemathicsApp.getControls().getRatio();
        iLeft = iRight = 0;
        timesLeft = new double[waypointsLeft.size()+1];
        double lmax = 0;
        double rmax = 0;
        double max = 0;
        for (double waypoint : waypointsLeft) {
            lmax = Math.max(lmax, waypoint);
            rmax = Math.max(rmax, waypoint/ratio);
        }
        for (double waypoint : waypointsRight) {
            rmax = Math.max(rmax, waypoint);
        }
        max = Math.max(max, lmax);
        max = Math.max(max, rmax);
        max = Math.ceil(max);
        correction = Math.max(lmax, lmax/ratio) / max;
        timesLeft[0] = 0.0;
        int i=1;
        for (double waypoint : waypointsLeft) {
            timesLeft[i++] = waypoint / lmax;
        }
        if (WiiKinemathicsApp.getControls().constantDriverOption.isSelected()) {
            timesRight = new double[waypointsRight.size()+1];
            timesRight[0] = 0.0;
            i=1;
            for (double waypoint : waypointsRight) {
                timesRight[i++] = waypoint / rmax;
            }
            times = timesRight;
        } else {
            times = timesRight = timesLeft;
        }
        multipliers = new double[times.length];
        cumulative = new double[times.length];
        cumulative[0] = 0.0;
        i=0;
        for (double waypoint : waypointsRight) {
            cumulative[i+1] = waypoint / lmax;
            multipliers[i] = cumulative[i+1] - cumulative[i];
            multipliers[i] /= times[i+1] - times[i];
            i++;
        }
        WiiKinemathicsApp.getControls().unitsSlider.setValue((int)max);
    }

    public double getCorrected(double t) {
        return t*correction;
    }

    public double getCumulative(double t, double ratio) {
        double val = ratio*(cumulative[iRight] + (t - times[iRight])*multipliers[iRight]);
        val *= correction;
        iLeft = Math.min(iLeft, timesLeft.length-2);
        iRight = Math.min(iRight, timesRight.length-2);
        if (t > timesLeft[iLeft+1]) {
            iLeft++;
            WiiKinemathicsApp.getApplication().motionPanel.addLeftWaypoint(t*ratio*correction);
        }
        if (t > timesRight[iRight+1]) {
            iRight++;
            WiiKinemathicsApp.getApplication().motionPanel.addRightWaypoint(val);
        }
        return val;
    }

    public String getLeftLabel(int i) {
        return String.valueOf(waypointsLeft.get(i));
    }

    public String getRightLabel(int i) {
        return String.valueOf(waypointsRight.get(i));
    }

    public boolean validate() {
        if (!getWaypoints()) return false;
        if (waypointsLeft.size() != waypointsRight.size()) return false;
        init();
        return true;
    }
}
